﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace UYD_WCS_AS_AGV_Controller.AGV
{
    public class AgvMsgHelper
    {
        public string GetSendTaskPublishInitCode()
        {
            Model.TaskPublishInit SendTaskPulishInitCode = new Model.TaskPublishInit()
            {
                op = "advertise",
                topic = "/flexbe/execute_behavior/goal",
                type = "flexbe_msgs/BehaviorExecutionActionGoal"
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }



        public string GetSendTaskPublishCode(List<string> locSiteList)
        {

            Model.HeaderStamp headStamp = new Model.HeaderStamp();
            headStamp.secs = 0;
            headStamp.nsecs = 0;
            Model.Header header = new Model.Header();
            header.seq = 0;
            header.stamp = headStamp;
            header.frame_id = "";



            Model.Goal_id_Stamp goalidstamp = new Model.Goal_id_Stamp();
            goalidstamp.secs = 0;
            goalidstamp.nsecs = 0;
            Model.Goal_id goalid = new Model.Goal_id();
            goalid.id = "";
            goalid.stamp = goalidstamp;

            Model.Goal goal = new Model.Goal();
            goal.behavior_name = "auto_task";
            goal.arg_keys = new List<string>();
            goal.arg_keys.Add("");
            goal.arg_values = new List<string>();
            goal.arg_values.Add("");
            goal.input_keys = new List<string>();
            goal.input_keys.Add("from_site");
            goal.input_values = locSiteList;

            Model.Msg msg = new Model.Msg();
            msg.header = header;
            msg.goal = goal;
            msg.goal_id = goalid;

            Model.TaskPublish taskPublish = new Model.TaskPublish();
            taskPublish.op = "publish";
            taskPublish.topic = "/flexbe/execute_behavior/goal";
            taskPublish.msg = msg;

            string strCode= Newtonsoft.Json.JsonConvert.SerializeObject(taskPublish);

            return strCode;
        }


        public string GetSendTaskPublish_RotateCode(string behavior_name)
        {

            Model.HeaderStamp headStamp = new Model.HeaderStamp();
            headStamp.secs = 0;
            headStamp.nsecs = 0;
            Model.Header header = new Model.Header();
            header.seq = 0;
            header.stamp = headStamp;
            header.frame_id = "";



            Model.Goal_id_Stamp goalidstamp = new Model.Goal_id_Stamp();
            goalidstamp.secs = 0;
            goalidstamp.nsecs = 0;
            Model.Goal_id goalid = new Model.Goal_id();
            goalid.id = "";
            goalid.stamp = goalidstamp;

            Model.Goal goal = new Model.Goal();
            goal.behavior_name = behavior_name;
            goal.arg_keys = new List<string>();
            goal.arg_keys.Add("");
            goal.arg_values = new List<string>();
            goal.arg_values.Add("");
            goal.input_keys = new List<string>();
            goal.input_keys.Add("from_site");
            goal.input_keys.Add("des_site");
            goal.input_values = new List<string>();
            goal.input_values.Add("");
            goal.input_values.Add("");

            Model.Msg msg = new Model.Msg();
            msg.header = header;
            msg.goal = goal;
            msg.goal_id = goalid;

            Model.TaskPublish taskPublish = new Model.TaskPublish();
            taskPublish.op = "publish";
            taskPublish.topic = "/flexbe/execute_behavior/goal";
            taskPublish.msg = msg;

            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(taskPublish);

            return strCode;
        }


        public string GetSendTaskReportInitCode()
        {
            Model.TaskReportInit SendTaskPulishInitCode = new Model.TaskReportInit()
            {
                op = "subscribe",
                topic = "/flexbe/behavior_updated",
                type = "std_msgs/String",
                queue_length=1
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }

        public Model.TaskReport GetTaskReport(string strReceiveMsg)
        {
            try
            {
                Model.TaskReport tmpTaskReport= Newtonsoft.Json.JsonConvert.DeserializeObject<Model.TaskReport>(strReceiveMsg);
                return tmpTaskReport;
            }
            catch(Exception exc)
            {
                return null;
            }
        }



        public string GetBatteryReportInitCode()
        {
            Model.BatteryReportInit SendTaskPulishInitCode = new Model.BatteryReportInit()
            {
                op = "subscribe",
                topic = "/battery",
                type = "sensor_msgs/BatteryState"
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }



        public string GetAlarmReportInitCode()
        {
            Model.AlarmReportInit SendTaskPulishInitCode = new Model.AlarmReportInit()
            {
                op = "subscribe",
                topic = "/diagnostics_agg",
                type = "diagnostic_msgs/DiagnosticArray"
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }

        public string GetPositionReportInitCode()
        {
            Model.PositionReportInit SendTaskPulishInitCode = new Model.PositionReportInit()
            {
                op = "subscribe",
                topic = "/robot_pose",
                type = "geometry_msgs/Pose"
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }


        public string GetTaskCancleCode()
        {
            Model.TaskCancel SendTaskPulishInitCode = new Model.TaskCancel()
            {
                op = "publish",
                topic = "/flexbe/command/preempt",
                msg = new Model.TaskCancelMsg() { }
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }

        public string GetTaskPauseRestoreInitCode()
        {
            Model.TaskPauseRestoreInit SendTaskPulishInitCode = new Model.TaskPauseRestoreInit()
            {
                op = "advertise",
                topic = "/flexbe/command/pause",
                type= "std_msgs/Bool"
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }
        public string GetTaskPauseRestoreCode(bool pauseOrRestore)
        {
            Model.TaskPauseRestore SendTaskPulishInitCode = new Model.TaskPauseRestore()
            {
                op = "publish",
                topic = "/flexbe/command/pause",
                msg = new Model.TaskPauseRestoreMsg() { data = pauseOrRestore }
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendTaskPulishInitCode);
            return strCode;
        }

        /// <summary>
        /// 获取最后到达站点
        /// </summary>
        /// <returns></returns>
        public string GetSiteReportInitCode()
        {
            Model.SiteReportInit SendSiteReportInitCode = new Model.SiteReportInit()
            {
                op = "subscribe",
                topic = "/site_feedback",
                type = "std_msgs/String",
                queue_length =1
            };
            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(SendSiteReportInitCode);
            return strCode;
        }



        public string GetSendTaskPublishCode(string behavior_name, List<string> listInputKey, List<string> listInputValue)
        {

            Model.HeaderStamp headStamp = new Model.HeaderStamp();
            headStamp.secs = 0;
            headStamp.nsecs = 0;
            Model.Header header = new Model.Header();
            header.seq = 0;
            header.stamp = headStamp;
            header.frame_id = "";



            Model.Goal_id_Stamp goalidstamp = new Model.Goal_id_Stamp();
            goalidstamp.secs = 0;
            goalidstamp.nsecs = 0;
            Model.Goal_id goalid = new Model.Goal_id();
            goalid.id = "";
            goalid.stamp = goalidstamp;

            Model.Goal goal = new Model.Goal();
            goal.behavior_name = behavior_name;
            goal.arg_keys = new List<string>();
            goal.arg_keys.Add("");
            goal.arg_values = new List<string>();
            goal.arg_values.Add("");
            goal.input_keys = listInputKey; 
            goal.input_values = listInputValue;

            Model.Msg msg = new Model.Msg();
            msg.header = header;
            msg.goal = goal;
            msg.goal_id = goalid;

            Model.TaskPublish taskPublish = new Model.TaskPublish();
            taskPublish.op = "publish";
            taskPublish.topic = "/flexbe/execute_behavior/goal";
            taskPublish.msg = msg;

            string strCode = Newtonsoft.Json.JsonConvert.SerializeObject(taskPublish);

            return strCode;
        }
    }
}
